/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-20 19:34:16
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-10 01:19:58
 * @FilePath: /esp32_motor/motor/current/currentSensing.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "delay.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "math_common.h"
#include "currentSensing.h"
#include "adc.h"

#define CURRENT_CORRECT_NUM 500

typedef struct
{
    int   voltage; /*单位mV*/
    _iq24 ampere;  /*单位A*/
} CurrentData;

typedef struct
{
    int   pin[MOTOR_PHASE_NUM - 1];            /*检测管脚，目前只需要两个*/
    int   voltageGain;                         /*电流检测芯片的电压增益*/
    int   offsetVoltage;                       /*电路的偏移电压mV*/
    int   correctVoltage[MOTOR_PHASE_NUM - 1]; /*校准电压，当输入为0时，电路的漂移电压*/
    _iq24 gain;
    float ohm; /*电阻大小*/
    // FilterAlgorithm   *filter[MOTOR_PHASE_NUM];             /*滤波对象*/
    // esp_timer_handle_t timerHandle;
    CurrentData data[MOTOR_PHASE_NUM]; /*电流数据*/
} CurrentObj;

static char *tag = "current";

/*检测电流*/
static esp_err_t CURRENT_readAll(CurrentSensing *dev)
{
    CurrentObj *priv = NULL;
    AdcData     data[MOTOR_PHASE_NUM - 1];
    _iq24       ampereSum = 0;

    if (!dev)
    {
        return ESP_FAIL;
    }

    priv = dev->priv;

    for (int i = 0; i < MOTOR_PHASE_NUM - 1; i++)
    {
        data[i] = ADC_getData(priv->pin[i]);
        /*计算电流大小*/
        priv->data[i].voltage = data[i].voltage - priv->correctVoltage[i];
        priv->data[i].ampere  = _IQmpyI32(priv->gain, priv->data[i].voltage);
        ampereSum             += priv->data[i].ampere;
    }

    /*用前两路电流计算第三路电流*/
    priv->data[MOTOR_PHASE_NUM - 1].ampere = 0 - ampereSum;

    return ESP_OK;
}
static _iq24 CURRENT_getIa(CurrentSensing *dev)
{
    CurrentObj *priv = NULL;
    int         index;

    if (!dev)
    {
        return ESP_FAIL;
    }

    priv = dev->priv;

    /*用滑动窗口滤波处理下数据*/
    index = 0;
    /*计算电流大小*/
    // priv->data[index].voltage = priv->data[index].voltage - priv->correctVoltage[index];
    // float sensingVolage       = ((float)(priv->data[index].voltage) / (float)priv->voltageGain) / 1000;
    // priv->data[index].ampere  = sensingVolage / priv->ohm * -1;
    return priv->data[index].ampere;
}
static _iq24 CURRENT_getIb(CurrentSensing *dev)
{
    CurrentObj *priv = NULL;
    int         index;

    if (!dev)
    {
        return ESP_FAIL;
    }

    priv = dev->priv;

    /*用滑动窗口滤波处理下数据*/
    index = 1;
    /*计算电流大小*/
    // priv->data[index].voltage = priv->data[index].voltage - priv->correctVoltage[index];
    // float sensingVolage       = ((float)(priv->data[index].voltage) / (float)priv->voltageGain) / 1000;
    // priv->data[index].ampere  = sensingVolage / priv->ohm * -1;
    return priv->data[index].ampere;
}
static _iq24 CURRENT_getIc(CurrentSensing *dev)
{
    CurrentObj *priv = NULL;

    if (!dev)
    {
        return ESP_FAIL;
    }

    priv = dev->priv;

    /*用滑动窗口滤波处理下数据*/
    return priv->data[2].ampere;
}

/*校准电压值*/
static void CURRENT_correctAmpere(CurrentSensing *dev)
{
    CurrentObj *priv = dev->priv;
    AdcData     data[MOTOR_PHASE_NUM - 1];
    int         correct[MOTOR_PHASE_NUM - 1] = {0, 0};

    mdelay(1000);
    /*采集50个点的0点电压*/
    for (int i = 0; i < CURRENT_CORRECT_NUM; i++)
    {
        mdelay(1);
        // CURRENT_readAll(dev);
        for (int j = 0; j < MOTOR_PHASE_NUM - 1; j++)
        {
            data[j] = ADC_getData(priv->pin[j]);
            // printf("out mean = %f\n",out);
            // printf("out mean = %f\n", out);
            correct[j] += data[j].voltage;
        }
    }

    for (int i = 0; i < MOTOR_PHASE_NUM - 1; i++)
    {
        priv->correctVoltage[i] = correct[i] / CURRENT_CORRECT_NUM;
        printf("correctVolate[%d]=%d\n", i, priv->correctVoltage[i]);
    }
}


/*删除电流检测*/
void CURRENT_delete(CurrentSensing *dev)
{
    CurrentObj *priv = NULL;

    if (!dev)
    {
        return;
    }
    priv = dev->priv;

    if (priv)
    {
        // for (int i = 0; i < MOTOR_PHASE_NUM; i++)
        // {
        //     FILTER_delete(priv->filter[i]);
        // }
        free(priv);
    }

    free(dev);
}

/*创建电流检测*/
esp_err_t CURRENT_create(CurrentSensing **dev, int pinA, int pinB, int voltageGain, int offsetVoltage, float ohm)
{
    CurrentObj     *priv = NULL;
    CurrentSensing *cur  = NULL;

    cur                  = calloc(1, sizeof(CurrentSensing));
    if (!cur)
    {
        return ESP_FAIL;
    }
    priv = calloc(1, sizeof(CurrentObj));
    if (!cur)
    {
        goto free;
    }
    cur->priv = priv;

    priv->pin[0]        = pinA;
    priv->pin[1]        = pinB;
    priv->ohm           = ohm;
    priv->voltageGain   = voltageGain;
    priv->offsetVoltage = offsetVoltage;
    priv->gain          = _IQ(-1.F / (float)voltageGain / ohm / 1000);

    /*注册adc管脚*/
    for (int i = 0; i < MOTOR_PHASE_NUM - 1; i++)
    {
        ADC_register(priv->pin[i]);
    }

    cur->readAll       = CURRENT_readAll;
    cur->getIa         = CURRENT_getIa;
    cur->getIb         = CURRENT_getIb;
    cur->getIc         = CURRENT_getIc;
    cur->correctAmpere = CURRENT_correctAmpere;

    // /*定时器*/
    // const esp_timer_create_args_t periodic_timer_args = {
    //     .callback = CURRENT_timerHandle,
    //     .arg      = cur,
    //     .dispatch_method = ESP_TIMER_ISR,
    //     /* name is optional, but may help identify the timer when debugging */
    //     .name = "periodic"};
    // esp_timer_create(&periodic_timer_args, &priv->timerHandle);
    // esp_timer_start_periodic(priv->timerHandle, 200);
    /*赋值*/
    *dev = cur;

    return ESP_OK;

free:
    CURRENT_delete(cur);
    return ESP_FAIL;
}
